#include <time.h>
#include <stdlib.h>
#include <nds.h>
#include <nds/arm9/console.h> //basic print funcionality
#include <stdio.h>
#include <math.h>
#define MAX_PLAYERS 4
#define PI 3.14159265
#define RED RGB15(31,0,0)
#define YELLOW RGB15(31,31,0)
#define BLUE RGB15(0,0,31)
#define GREEN RGB15(0,31,0)
#define PURPLE RGB15(31,0,31)
#define WHITE RGB15(31,31,31)
#define BLACK RGB15(0,0,0)
#define TURNSPEED 4
#define FIELD_OFFSET 2

// globals

unsigned char playfield[SCREEN_WIDTH][SCREEN_HEIGHT];

void clearscreen(){
	
	uint16* framebuffer = VRAM_A; 
	
	for(int j=0;j<SCREEN_HEIGHT*SCREEN_WIDTH;j++)
		*framebuffer++ = BLACK;
}

class Player{
	private:
	int _id;
	
	bool _hole;
	int _holeamount;
	
	float _xpos;
	float _ypos;

	int _x;
	int _y;
	
	int _oldx;
	int _oldy;
	public:
	Player(int id);
	~Player();
	
	void checkcollision();
	void updatedir();
	void updatepos();
	void draw();
	void newlevel();
	void die();
	void win();
	bool hole();

	private:
	u16 turn_left;
	u16 turn_right;
	bool alive;

	uint16 color;
		
	float dir_x;
	float dir_y;
	u16 angle;
};


Player* players[MAX_PLAYERS];

int number_of_players;
int players_alive;

int fps = 0;

void clearScreen();
void reset();
void drawLevel();

int time1;
int time2;

int main(void)
{
	int j;
	number_of_players = 4;//MAX_PLAYERS; // assign in menu later

	powerON(POWER_ALL);
	
	irqInit();
	irqEnable(IRQ_VBLANK);
	
	srand(IPC->unixTime);
	
	videoSetMode(MODE_FB0);

	videoSetModeSub(MODE_0_2D | DISPLAY_BG0_ACTIVE); //sub bg 0 will be used to print text

    // set the first bank as background memory and the third as sub background memory
    // B and D are not used (if you want a bitmap greater than 256x256 you will need more
    // memory so another vram bank must be used and mapped consecutivly
    vramSetMainBanks(	VRAM_A_MAIN_BG_0x06000000, VRAM_B_LCD,
						VRAM_C_SUB_BG , VRAM_D_LCD);

	vramSetBankA(VRAM_A_LCD);

	// set up text background for text
    SUB_BG0_CR = BG_MAP_BASE(31);

	BG_PALETTE_SUB[255] = WHITE;//by default font will be rendered with color 255

	//consoleInit() is a lot more flexible but this gets you up and running quick
	consoleInitDefault((u16*)SCREEN_BASE_BLOCK_SUB(31), (u16*)CHAR_BASE_BLOCK_SUB(0), 16);
	
	iprintf("\t\tAchtung die Kurve \n\t\tfor Nintendo DS\n\n");
	iprintf("Press the Left and Right shoul-\nder button at the same time, to start and reset the game\n");
	
	for(j=0;j<number_of_players; j++){
		players[j] = new Player(j+1);
	}
	clearscreen();
	reset();
	drawLevel();

	int frames =0;
	int frame_speed = 10; // put 2 for slow - 8/10 for medium- 15 for fast
	while(1){
		scanKeys ();

		for(j=0; j< number_of_players; j++){
			players[j]->updatedir();
		}
		
		for(j=0; j< number_of_players; j++){
			players[j]->updatepos();
		}
		
		for(j=0; j< number_of_players; j++){
			if(!players[j]->hole()){
				players[j]->draw();
			}
		}
		
		if(players_alive <= 1){
			for(j=0;j<number_of_players;j++){
				players[j]->win();
			}
			reset();
			drawLevel();	
		}
		swiWaitForVBlank();
		if(++frames >=frame_speed){
			swiWaitForVBlank();
			frames=0;
		}
	
	
	time2= time1;
	time1= IPC->time.rtc.seconds;
	
	if(time1 != time2){
			
			printf("\x1b[18;0H");
			printf("Frames/sec: %d",fps);
			fps = 0;
	}else{
		fps++;
	}

	}
}

void Player::die(){
	if(alive){ // can't die when you're dead
		alive = false;
		players_alive--;
		//iprintf("Snake %d Died @ %d,%d/%d\n", _id, _x, _y, angle);
	}
}

void Player::win(){
	if(alive)
		iprintf("\nGo snake %d, you have won!\n",_id);
	
}

Player::Player(int id){
alive = true;

_id = id;
_holeamount = 15+rand()%25;
_hole = false;
newlevel();

}



void Player::checkcollision(){
	
	u16 new_x=(int) round(_xpos+dir_x);
	u16 new_y=(int) round(_ypos+dir_y);
	
	if(new_x < FIELD_OFFSET+2 || new_x > SCREEN_WIDTH - FIELD_OFFSET -4){ // out of bounds
		_x = new_x;
		_y = new_y;
		//draw(); // or not?
		die();	
	}else if(new_y < FIELD_OFFSET+2 || new_y > SCREEN_HEIGHT - FIELD_OFFSET-4){ // out of bounds
		_x = new_x;
		_y = new_y;
		//draw(); // or not?
		die();	
	}else if(!(_x == new_x && _y == new_y)){ // if one of the two has changed check for collison
		
		if(new_y == _y){
		
			if(new_x > _x){
				if(playfield[new_x+1][new_y] == 2 || playfield[new_x+1][new_y+1] == 2){ //if only x has increased
					_x = new_x;
					_y = new_y;
					//draw(); // or not?
					die();
				}
			}else if(new_x < _x){
				if(playfield[new_x-1][new_y] == 2 || playfield[new_x][new_y] == 2){ //if only x has decreased
					_x = new_x;
					_y = new_y;
					draw(); // or not?
					die();
				}
			}
			
		}else if(new_x == _x){
		
			if(new_y > _y){
				if(playfield[new_x][new_y+1] == 2 || playfield[new_x+1][new_y+1] == 2){ //if only y has increased
					_x = new_x;
					_y = new_y;
					//draw(); // or not?
					die();
				}
			}else if(new_y < _y){
				if(playfield[new_x][new_y-1] == 2 || playfield[new_x+1][new_y-1] == 2){ //if only y has increased
					_x = new_x;
					_y = new_y;
					draw(); // or not?
					die();
				}
			}
		
		}else if(new_y > _y){
		
			if(new_x > _x){
				if(playfield[new_x+1][new_y] == 2 || playfield[new_x+1][new_y+1] == 2 || playfield[new_x][new_y+1] == 2 ){ //if y has increased and x increased
					_x = new_x;
					_y = new_y;
					//draw(); // or not?
					die();
				}
			}else if(new_x < _x){
				if(playfield[new_x-1][new_y] == 2 || playfield[new_x-1][new_y-1] == 2 || playfield[new_x][new_y-1] == 2 ){ //if y has increased and x decreased
					_x = new_x;
					_y = new_y;
					draw(); // or not?
					die();
				}
			}
		}else if(new_y < _y){
			if(new_x == 0){
				
			}	
			else if(new_x > _x){
				if(playfield[new_x][new_y] == 2 || playfield[new_x+1][new_y+1] == 2 || playfield[new_x+1][new_y] == 2 ){ //if y has decreased and x increased
					_x = new_x;
					_y = new_y; 
					draw(); // or not?
					die();
				}
			}else if(new_x < _x){
				if(playfield[new_x][new_y-1] == 2 || playfield[new_x-1][new_y] == 2 || playfield[new_x-1][new_y-1] == 2 ){ //if y has decreased and x decreased
					_x = new_x;
					_y = new_y;
					draw(); // or not?
					die();
				}
			}
		}
	}
}

void Player::updatepos(){
	
	_oldy = _y;
	_oldx = _x;
	
	if(alive){
		_xpos += dir_x;
		if(_xpos<0){
			_xpos=0;
			die();
		}
		if(_xpos>SCREEN_WIDTH-2){
			_xpos=SCREEN_WIDTH-2;
			die();
		}
		_ypos += dir_y;
		if(_ypos<0){
			_ypos=0;
			die();
		}
		
		if(_ypos>SCREEN_HEIGHT-2){
			_ypos=SCREEN_HEIGHT-2;
			die();
		}
		
		_x = (int) round(_xpos);
		_y = (int) round(_ypos);
		
		//iprintf("%03d, x=%03d, y=%03d\n", angle, _x, _y);
	}
}

void Player::updatedir(){
	if(alive){
		if(keysHeld() & turn_left)
			angle+=TURNSPEED;
	
		if(keysHeld() & turn_right)
			angle-=TURNSPEED;
		
		if(angle <= TURNSPEED -1)
			angle = 256;
		if(angle > 511)
			angle = 256;
	
		dir_x = cos((angle)*(PI/128));
		dir_y = sin((angle)*(PI/128));

		if(!_hole){
			checkcollision(); // check if they gonna hit something
		}
	}
}

void Player::draw(){
		// set to screen x=0 and y=0
		uint16* framebuffer = VRAM_A; 
		
		if(alive){
			// output the curve to the screen, with 2x2 blocks
			framebuffer += SCREEN_WIDTH*_y+_x; 		
			*framebuffer = color;
			framebuffer++;
			*framebuffer = color; 
			framebuffer += SCREEN_WIDTH;
			*framebuffer = color; 
			framebuffer--;
			*framebuffer = color;

			//update the array with the available playfield also a 2x2 block
			playfield[_x][_y] = 2; //indicates the field has been occupied by a curve
			playfield[_x][_y+1] = 2; //indicates the field has been occupied by a curve
			playfield[_x+1][_y] = 2; //indicates the field has been occupied by a curve
			playfield[_x+1][_y+1] = 2; //indicates the field has been occupied by a curve
			
		}
}


void reset(){
	
	
	int j;
	int k;
	for(j=0;j<SCREEN_WIDTH;j++)
		for(k=0;k<SCREEN_HEIGHT;k++)
			playfield[j][k] = 1; // no-one has been there yet
	
	while(!(keysHeld() & KEY_L && keysHeld() & KEY_R) && !(keysDown() & KEY_TOUCH)){
		scanKeys();
		if(keysDown() & KEY_START){
			switch(number_of_players){
				case 2:	number_of_players = 3;
				break;
				case 3:number_of_players = 4;
				break;
				default: number_of_players = 2;
			}
		}
		printf("\x1b[10;0H");
		printf("Number of players: %d",number_of_players);
	}
	
	clearscreen();
	
	for(j=0;j<number_of_players; j++)
		players[j]->newlevel();
		
	players_alive = number_of_players;
}

void Player::newlevel(){
	alive = true;
	
_xpos = 30 + rand()%((SCREEN_WIDTH-2*30)/4) *4;
_ypos = 30 + rand()%((SCREEN_HEIGHT-2*30)/4) *4;

angle = 256 + rand()%(256/4) *4;  // angle has to be at least TURNSPEED or larger when it starts

//printf("Snake %d started at %d,%d/%d\n",_id,_x,_y,angle);

switch(_id){
	case 1: 
				color = RED;
				
				turn_right = KEY_LEFT + KEY_RIGHT;
				turn_left = KEY_DOWN + KEY_UP;
				break;
	case 2:
				color = YELLOW;
				
				turn_left = KEY_A;
				turn_right = KEY_B;
				break;
				
	case 3:
				color = GREEN;
				
				turn_left = KEY_START;
				turn_right = KEY_SELECT;
				break;
				
	case 4:
				color = BLUE;
				
				turn_left = KEY_X;
				turn_right = KEY_Y;
				break;
	
	default: break;
	}
	_x = (int) round(_xpos);
	_y = (int) round(_ypos);	
}

void drawLevel(void){
	uint16* framebuffer = VRAM_A + FIELD_OFFSET * SCREEN_WIDTH + FIELD_OFFSET;
	int i;

	for(i = 0; i < SCREEN_WIDTH-2*FIELD_OFFSET; i++){
		*framebuffer++ = WHITE;
	}
	framebuffer += 2*FIELD_OFFSET;
	for(i = 0; i < SCREEN_WIDTH-2*FIELD_OFFSET; i++){
		*framebuffer++ = WHITE;
	}
	
	framebuffer += 2*FIELD_OFFSET;
	for(i=0;i<SCREEN_HEIGHT-2*FIELD_OFFSET-4;i++){
		*framebuffer = WHITE;
		framebuffer++;
		*framebuffer++ = WHITE;
		framebuffer += (SCREEN_WIDTH - 2*FIELD_OFFSET - 4);
		
		*framebuffer = WHITE;
		framebuffer++;
		*framebuffer = WHITE;
		framebuffer++;
		
		framebuffer += 2*FIELD_OFFSET;
	}
	for(i = 0; i < SCREEN_WIDTH-2*FIELD_OFFSET; i++){
		*framebuffer++ = WHITE;
	}
	framebuffer += 2*FIELD_OFFSET;
	for(i = 0; i < SCREEN_WIDTH-2*FIELD_OFFSET; i++){
		*framebuffer++ = WHITE;
	}
}

bool Player::hole(){	
	if(_holeamount==0){
		if(!_hole && (rand()%10)>2){ //30% chance to enter hole mode
			//enter hole-mode
			_holeamount = 3+(rand() % 3);
			return _hole = true;
		}else{
			//not in hole-mode anymore
			_holeamount = 15+(rand() % 30);
			checkcollision();
			return _hole = false;
		}
	}
	_holeamount--;
	return _hole;
}
